/*
 * tension.c
 *
 *  Created on: Oct 14, 2024
 *      Author: Jinming
 */
#include "sensor.h"
#include <string.h>
#include "cmsis_os.h"
#include "task_info.h"
#include "can.h"
#include "can1.h"
#include "can2.h"

#define TAG		"sensor"

typedef union
{
	uint8_t data[4];
	int32_t value;
}TensionFormat_t;

typedef union
{
	uint8_t data[2];
	int16_t value;
}PlantarFormat_t;

static LegSensor_t gs_tLegSensor;

osThreadId_t g_tSensorTaskHandle[] = {NULL, NULL};
const osThreadAttr_t g_tSensorTaskAttr[] = {
	[0] = {
		.name = SENSOR1_TASK_NAME,
		.stack_size = 128 * 8,
		.priority = (osPriority_t) SENSOR1_TASK_PRIO},

	[1] = {
		.name = SENSOR2_TASK_NAME,
		.stack_size = 128 * 8,
		.priority = (osPriority_t) SENSOR2_TASK_PRIO},
};

osMutexId_t g_tForceMutex = NULL;
const osMutexAttr_t g_tForceMutexAttr = {
  .name = "force"
};

int32_t tension_convert(uint8_t *pData)
{
	TensionFormat_t tFormat;

	tFormat.data[0] = pData[0];
	tFormat.data[1] = pData[1];
	tFormat.data[2] = pData[2];
	tFormat.data[3] = pData[3];

	return tFormat.value;
}

int16_t plantar_convert(uint8_t *pData)
{
	PlantarFormat_t tFormat;

	tFormat.data[0] = pData[1];
	tFormat.data[1] = pData[0];

	return tFormat.value;
}

void tension_info_decode(uint32_t id, uint8_t *pData)
{
	if(id == 0x181)
	{
		gs_tLegSensor.force[0] = tension_convert(&pData[0]);
		gs_tLegSensor.force[1] = tension_convert(&pData[4]);
	}
	else if(id == 0x281)
	{
		gs_tLegSensor.force[2] = tension_convert(&pData[0]);
		gs_tLegSensor.force[3] = tension_convert(&pData[4]);
	}
	else if(id == 0x381)
	{
		gs_tLegSensor.force[4] = tension_convert(&pData[0]);
		gs_tLegSensor.force[5] = tension_convert(&pData[4]);
	}
//		LOGD(TAG, "%ld\t%ld\t%ld\t%ld\t%ld\t%ld", gs_tLegSensor.force[0],
//				gs_tLegSensor.force[1], gs_tLegSensor.force[2], gs_tLegSensor.force[3],
//				gs_tLegSensor.force[4], gs_tLegSensor.force[5]);
}

void plantar_info_decode(PlantarInfo_t *plantar, uint8_t *pData)
{
	uint8_t sensorId = pData[0];
	plantar[sensorId].x = plantar_convert(&pData[1]);
	plantar[sensorId].y = plantar_convert(&pData[3]);
	plantar[sensorId].z = plantar_convert(&pData[5]);

//	LOGD(TAG, "plantar%d, %d\t%d\t%d", sensorId, gs_tLegSensor.plantarL[sensorId].x,
//			gs_tLegSensor.plantarL[sensorId].y, gs_tLegSensor.plantarL[sensorId].z);
}

void Sensor0_task(void *param)
{
	CANMsgRx_t tFrame;
	osStatus_t osStatus;

	can1_rx_clear();
	while(1)
	{
		can1_read(&tFrame, portMAX_DELAY);

		osStatus = osMutexAcquire(g_tForceMutex, 10);
		if(osStatus)
		{
			LOGD(TAG, "Write:Can not acquire tension mutex");
			continue;
		}
		can1_rx_clear();

		if((tFrame.id == 0x181) || (tFrame.id == 0x281) || (tFrame.id == 0x381))
		{
			tension_info_decode(tFrame.id, tFrame.data);
		}
		else if((tFrame.id & 0x7) == 4)
		{
			plantar_info_decode(gs_tLegSensor.plantarL, tFrame.data);
		}
		else if((tFrame.id & 0x7) == 5)
		{
			plantar_info_decode(gs_tLegSensor.plantarR, tFrame.data);
		}

		osMutexRelease(g_tForceMutex);
	}
}

void Sensor1_task(void *param)
{
	CANMsgRx_t tFrame;
	osStatus_t osStatus;

	can2_rx_clear();
	while(1)
	{
		can2_read(&tFrame, portMAX_DELAY);

		osStatus = osMutexAcquire(g_tForceMutex, 10);
		if(osStatus)
		{
			LOGD(TAG, "Write:Can not acquire tension mutex");
			continue;
		}

		can2_rx_clear();

		if((tFrame.id == 0x181) || (tFrame.id == 0x281) || (tFrame.id == 0x381))
		{
			tension_info_decode(tFrame.id, tFrame.data);
		}
		else if((tFrame.id & 0x7) == 4)
		{
			plantar_info_decode(gs_tLegSensor.plantarL, tFrame.data);
		}
		else if((tFrame.id & 0x7) == 5)
		{
			plantar_info_decode(gs_tLegSensor.plantarR, tFrame.data);
		}

		osMutexRelease(g_tForceMutex);
	}
}

int sensor_get(struct spi_sensor_data_t *sensor)
{
	osStatus_t osStatus;
	osStatus = osMutexAcquire(g_tForceMutex, 10);
	if(osStatus)
	{
		LOGD(TAG, "Read:Can not acquire tension mutex");
		return -1;
	}

	for(uint8_t i = 0; i < sizeof(gs_tLegSensor.force)/sizeof(gs_tLegSensor.force[0]); i++)
	{
		sensor->force[i] = gs_tLegSensor.force[i];
	}

	for(uint8_t i = 0; i < sizeof(gs_tLegSensor.plantarL)/sizeof(gs_tLegSensor.plantarL[0]); i++)
	{
		sensor->plantarL[i * 3] = gs_tLegSensor.plantarL[i].x;
		sensor->plantarL[i * 3 + 1] = gs_tLegSensor.plantarL[i].y;
		sensor->plantarL[i * 3 + 2] = gs_tLegSensor.plantarL[i].z;
//		LOGD(TAG, "plantar%d, %d\t%d\t%d", i, gs_tLegSensor.plantarL[i].x,
//				gs_tLegSensor.plantarL[i].y, gs_tLegSensor.plantarL[i].z);
	}

	for(uint8_t i = 0; i < sizeof(gs_tLegSensor.plantarR)/sizeof(gs_tLegSensor.plantarR[0]); i++)
	{
		sensor->plantarR[i * 3] = gs_tLegSensor.plantarR[i].x;
		sensor->plantarR[i * 3 + 1] = gs_tLegSensor.plantarR[i].y;
		sensor->plantarR[i * 3 + 2] = gs_tLegSensor.plantarR[i].z;
	}

	osMutexRelease(g_tForceMutex);

	return 0;
}

int sensor_config(void)
{
//	can1_unconfig();
//	can2_unconfig();
//	MX_CAN_DeInit(&hcan1);
//	MX_CAN_DeInit(&hcan2);
//
//	osDelay(500);
//	MX_CAN_Init_100k(&hcan1);
//	MX_CAN_Init_100k(&hcan2);
//	can1_config(NULL);
//	can2_config(NULL);
	can1_rx_clear();
	can2_rx_clear();

	if(osThreadGetState(g_tSensorTaskHandle[0]) != osThreadRunning)
	{
		osThreadResume(g_tSensorTaskHandle[0]);
	}

	if(osThreadGetState(g_tSensorTaskHandle[1]) != osThreadRunning)
	{
		osThreadResume(g_tSensorTaskHandle[1]);
	}

	return 0;
}

int sensor_unconfig(void)
{
//	can1_unconfig();
//	can2_unconfig();
//	MX_CAN_DeInit(&hcan1);
//	MX_CAN_DeInit(&hcan2);
//
//	osDelay(500);
//	MX_CAN1_Init();
//	MX_CAN2_Init();
//	can1_config(NULL);
//	can2_config(NULL);

	if(osThreadGetState(g_tSensorTaskHandle[0]) != osThreadBlocked)
	{
		osThreadSuspend(g_tSensorTaskHandle[0]);
	}

	if(osThreadGetState(g_tSensorTaskHandle[1]) != osThreadBlocked)
	{
		osThreadSuspend(g_tSensorTaskHandle[1]);
	}

	return 0;
}

int sensor_init(void)
{
	memset(&gs_tLegSensor, 0, sizeof(LegSensor_t));
//	can1_unconfig();
//	can2_unconfig();
//	MX_CAN_DeInit(&hcan1);
//	MX_CAN_DeInit(&hcan2);
//
//	osDelay(500);
//	MX_CAN_Init_100k(&hcan1);
//	MX_CAN_Init_100k(&hcan2);
//	can1_config(NULL);
//	can2_config(NULL);

	g_tForceMutex = osMutexNew(&g_tForceMutexAttr);
	if(g_tForceMutex == NULL)
	{
		LOGE(TAG, "Tension mutex create error");
		return -1;
	}

	g_tSensorTaskHandle[0] = osThreadNew(Sensor0_task, NULL, &g_tSensorTaskAttr[0]);
	if(g_tSensorTaskHandle[0] == NULL)
	{
		LOGE(TAG, "Tension1 task create error!!!");
		return -1;
	};
	osThreadSuspend(g_tSensorTaskHandle[0]);

	g_tSensorTaskHandle[1] = osThreadNew(Sensor1_task, NULL, &g_tSensorTaskAttr[1]);
	if(g_tSensorTaskHandle[1] == NULL)
	{
		LOGE(TAG, "Tension2 task create error!!!");
		return -1;
	};
	osThreadSuspend(g_tSensorTaskHandle[1]);

	LOGD(TAG, "tension task init ok");
	return 0;
}


